#ifndef UNIVERSAL_DIMENSION_CONVERTOR_H
#define UNIVERSAL_DIMENSION_CONVERTOR_H

#include "DimensionConvertor.h"

class UniversalDimensionConvertor : public DimensionConvertor{
public:
	struct univ_convert_ptr{
		unsigned long long fx;
		unsigned long long fy;
		int width, height;
		int cx;
		int cy;
		__host__ univ_convert_ptr(
			int x,
			int y,
			unsigned long long fx,
			unsigned long long fy,
			int cx,
			int cy){
				this->fx = fx;
				this->fy = fy;
				this->cx = cx;
				this->cy = cy;
				width = x;
				height = y;
		}

		 __host__ __device__ float3 operator()(float3 in){
			
			in.y = cy - in.y;			
			in.x = in.x - cx;
			
			in.x /= fx;
			in.y /= fy;

			in.x *= in.z;
			in.y *= in.z;
			return in;
		}		


		__host__ __device__ float3 operator()(thrust::tuple<float, int> in){
			float3 r;

			//Depth
			r.z = thrust::get<0>(in);

			//Decompose 1D index to (x, y)
			r.y = (thrust::get<1>(in) / width);
			r.x = (thrust::get<1>(in) % width);

			return operator()(r);
		}
	};


	struct univ_convert_rtp {
		unsigned long long fx;
		unsigned long long fy;
		int cx;
		int cy;
		int width;
		int height;
		
		__host__ univ_convert_rtp(
			unsigned long long fx,
			unsigned long long fy,
			int cx,
			int cy,
			int width,
			int height) {
				this->fx = fx;
				this->fy = fy;
				this->cx = cx;
				this->cy = cy;
				this->width = width;
				this->height = height;
		}

		__host__ __device__ float2 operator()(float3 in){
			float2 out;
			if(abs(in.z) < 1.0){
				out.x = -1.0;
				out.y = -1.0;
			} else {
				out.x = in.x / in.z;
				out.y = in.y / in.z;
				out.x *= fx;
				out.y *= fy;

				out.x = out.x + cx;
				out.y = cy - out.y;
			}
			return out;
		}
	};



	UniversalDimensionConvertor(){};
	~UniversalDimensionConvertor();


	virtual void setCameraParameters(
		const unsigned long long fx,
		const unsigned long long fy,
		const int cx,
		const int cy);

	virtual void projectiveToReal(float* data, float3* out, int x, int y, int size);
	virtual void projectiveToReal(float3* data, float3* out, int x = 320, int y = 240, int size = 320 * 240);
	virtual void realToProjective(float3* data, float* out, int x = 320, int y = 240);
	virtual void realToProjective(float3* data, float* weight, float2* out, int x = 320, int y = 240);
	void realToProjective2(float3* data, float2* out, int x, int y, int size);



private:
	//virtual void realToProjectiveHelper(float3* data, int x, int y);

	unsigned long long fx;
	unsigned long long fy;
	int cx;
	int cy;
};


#endif